#include "system.h"
#include "print.h"
#include "Sd2Card.h"

unsigned int millis(){
  return IORD(MYTIMER, MYTIMER_32);
}


int _readBlock(StorageDevice* self, unsigned int block, char* dst) {
  int n;
  int offset = 0;
  int count = 512;
  int* idst = (int*)dst;
  Sd2Card* _self = (Sd2Card*)self;
  //if (!inBlock_ || block != block_ || offset < offset_) {
    //block_ = block;
    // use address if not SDHC card
  if (_self->type_!= SD_CARD_TYPE_SDHC) block <<= 9;
  if (_self->cardCommand(CMD17, block)) {
    _self->error = SD_CARD_ERROR_CMD17;
    goto fail;
  }
  if (!_self->waitStartBlock()) {
    goto fail;
  }


  //for (int i = 0; i < 512; i++) {
  //  dst[i] = spiRec();
  //}
  
  for (int i = 0; i < 128; i++) {
    int val = spiRec32();
    //printHex(val);
    idst[i] = val;
  }

  _self->readEnd();
  //}
  return true;

 fail:
  _self->chipSelectHigh();
  return false;
}

int _writeBlock(StorageDevice* self, unsigned int blockNumber, const char* src) {
  // use address if not SDHC card
  Sd2Card* _self = (Sd2Card*)self;

  if (_self->type_ != SD_CARD_TYPE_SDHC) blockNumber <<= 9;
  if (_self->cardCommand(CMD24, blockNumber)) {
    _self->error = SD_CARD_ERROR_CMD24;
    goto fail;
  }
  if (!_self->writeData(DATA_START_BLOCK, src)) goto fail;

  // wait for flash programming to complete
  if (!_self->waitNotBusy(SD_WRITE_TIMEOUT)) {
    _self->error = SD_CARD_ERROR_WRITE_TIMEOUT;
    goto fail;
  }
  // response is r2 so get and check two bytes for nonzero
  if (_self->cardCommand(CMD13, 0) || spiRec()) {
    _self->error = SD_CARD_ERROR_WRITE_PROGRAMMING;
    goto fail;
  }
  _self->chipSelectHigh();
  return true;

 fail:
  _self->chipSelectHigh();
  return false;
}

void Sd2Card::readEnd(void) {
  //if (inBlock_) {
      // skip data and crc
    //while (offset_++ < 514) spiRec();
    spiRec();
    spiRec();
    chipSelectHigh();
    //inBlock_ = 0;
  //}
}

// private functions
int Sd2Card::cardAcmd(int cmd, unsigned int arg) {
  cardCommand(CMD55, 0);
  return cardCommand(cmd, arg);
}

void Sd2Card::chipSelectHigh() {
  SPI_CHIP_SELECT_HIGH();
}
void Sd2Card::chipSelectLow() {
  //SPI_WRITE_POLPHA(polpha_);
  SPI_CHIP_SELECT_LOW(chipSelectPin_);
}

int Sd2Card::waitNotBusy(int timeoutMillis) {
  unsigned int t0 = millis();
  do {
    if (spiRec() == 0XFF) return true;
  }
  while ((millis() - t0) < timeoutMillis);
  return false;
}

int Sd2Card::writeData(int token, const char* src) {
  spiSend(token);
  for (int i = 0; i < 512; i++) {
    spiSend(src[i]);
  }
  spiSend(0xff);  // dummy crc
  spiSend(0xff);  // dummy crc

  status_ = spiRec();
  if ((status_ & DATA_RES_MASK) != DATA_RES_ACCEPTED) {
    error = SD_CARD_ERROR_WRITE;
    chipSelectHigh();
    return false;
  }
  return true;
}

void Sd2Card::printType(){
  switch(type_){
    case 1:print("sd1\r\n");break;
    case 2:print("sd2\r\n");break;
    case 3:print("sdhc\r\n");break;
    case 4:print("mmc\r\n");break;
  }
}

///////////////////////////////////////////////////
int Sd2Card::cardCommand(int cmd, unsigned int arg) {
  // end read if in partialBlockRead mode
  readEnd();

  // select card
  chipSelectLow();

  // wait up to 300 ms if busy
  waitNotBusy(300);

  // send command
  spiSend(cmd | 0x40);

  int s;
  // send argument
  for (s = 24; s >= 0; s -= 8) spiSend(arg >> s);

  // send CRC
  int crc = 0XFF;
  if (cmd == CMD0) crc = 0X95;  // correct crc for CMD0 with arg 0
  if (cmd == CMD8) crc = 0X87;  // correct crc for CMD8 with arg 0X1AA
  spiSend(crc);

  // wait for response
  int i;
  for (i = 0; ((status_ = spiRec()) & 0X80) && i != 0XFF; i++)
    ;
  
  return status_;
}

int initError;
int Sd2Card::init(int chipSelectPin) {
  initError = 0;
  
  this->readBlock = _readBlock;
  this->writeBlock = _writeBlock;

  unsigned int arg;
  
  error = 0;
  //inBlock_ = 0;
  //partialBlockRead_ = 0;
  type_ = 0;
  
  //offset_=0;
  //IOWR(MYTIMER32, 0, 0);
  
  error = type_ = 0;//inBlock_ =  partialBlockRead_ = 
  chipSelectPin_ = chipSelectPin;
  // 16-bit init start time allows over a minute
  unsigned int t0 = millis();

  chipSelectHigh();
  
  // must supply min of 74 clock cycles with CS high.
  for (int i = 0; i < 20; i++) spiSend(0XFF);


  // command to go idle in SPI mode
  int ok = 0;
  //for(int i=0;i<16;i++){
  //  polpha_ = i;
    chipSelectLow();
    
    for(int i=0;i<10;i++){
      status_ = cardCommand(CMD0, 0);
      if(status_ == R1_IDLE_STATE){
        ok = 1;
        break;
      }else{
        print("status_1 ");printInt(status_);print("\r\n");
      }
      IOWR(MYTIMER, MYTIMER_16, 0);
      while(IORD(MYTIMER, MYTIMER_16)<1000);
    }
    //if(ok){
      //print("polpha=");printInt(polpha_);print("\r\n");
    //  break;
    //}
  //}
  if(!ok){
    initError = 1;
    goto fail;
  }
  //while ((status_ = cardCommand(CMD0, 0)) != R1_IDLE_STATE) {
  //  if (((int)(millis() - t0)) > SD_INIT_TIMEOUT) {
  //    initError = 1;
  //    goto fail;
  //  }
  //}
  // check SD version
  if ((cardCommand(CMD8, 0x1AA) & R1_ILLEGAL_COMMAND)) {


      IOWR(MYTIMER, MYTIMER_16, 0);
      while(true){
        status_ = cardCommand(CMD1, 0);
        if(status_ == R1_READY_STATE) {
          type_ = SD_CARD_TYPE_MMC;
          chipSelectHigh();
          return true;
        }
        if(IORD(MYTIMER, MYTIMER_16)>65534){//0.3s
          break;
        }
      }

    type_ = SD_CARD_TYPE_SD1;
  } else {
    // only need last byte of r7 response
    for (int i = 0; i < 4; i++) status_ = spiRec();
    if (status_ != 0XAA) {
      initError = 2;
      goto fail;
    }
    type_ = SD_CARD_TYPE_SD2;
  }
  // initialize card and send host supports SDHC if SD2
  arg = type_ == SD_CARD_TYPE_SD2 ? 0X40000000 : 0;

  while ((status_ = cardAcmd(ACMD41, arg)) != R1_READY_STATE) {
    print("status_3 ");printInt(status_);print("\r\n");
    // check for timeout
    if ((millis() - t0) > SD_INIT_TIMEOUT) {
      initError = 3;
      goto fail;
    }
  }
  // if SD2 read OCR register to check for SDHC card
  if (type_ == SD_CARD_TYPE_SD2) {
    if (cardCommand(CMD58, 0)) {
      initError = 4;
      goto fail;
    }
    if ((spiRec() & 0XC0) == 0XC0) type_ = SD_CARD_TYPE_SDHC;
    // discard rest of ocr - contains allowed voltage range
    for (int i = 0; i < 3; i++) spiRec();
  }
  chipSelectHigh();

  return true;

 fail:
  chipSelectHigh();
  return false;

};

int Sd2Card::waitStartBlock(void) {
  unsigned int t0 = millis();
  while ((status_ = spiRec()) == 0XFF) {
    if ((millis() - t0) > SD_READ_TIMEOUT) {
      initError = 5;
      goto fail;
    }
  }
  if (status_ != DATA_START_BLOCK) {
    initError = 6;
    goto fail;
  }
  return true;

 fail:
  chipSelectHigh();
  return false;
}
